#include <stdio.h>
#include <stdlib.h>  // needed for sysconf(int name);
#include <unistd.h>
#include <string.h>
#include <pthread.h>
#include <time.h>

#include <getopt.h>
#include <signal.h>
#include <sys/mman.h>  // Needed for mlockall()
#include <malloc.h>
#include <sys/time.h>      // needed for getrusage
#include <sys/resource.h>  // needed for getrusage
#include <sys/shm.h>
#include <limits.h>

#include "zuc/nml_intf/zucpos.h"

#include "common_def_sensor.h"
#include "torq_mem_operate.h"

#include "libForceSensor.h"

#define MAX_SUPPORT_BRAND 8

#define MY_STACK_SIZE (4 * 1024 * 1024) /* 100 kB is enough for now. */

SensorShm* sensorShmStruct = NULL;
static int torqSensorKey = 201;

static ForceSensor* sensor = nullptr;

static int quit_flag = 0;
static int hal_mod_id = 0;

static int last_mode = 0;

static double FT_last[CART_DIM] = {0};
static double filter = 0.0;

static void windowFilter(const int w, const double* FT_in, double* FT_out)
{
    static double _FT_in_last[11][CART_DIM] = {0};
    if (w > 10)
    {
        printf("Error: N is larger than 10!");
        return;
    }
    for (int i = 0; i < CART_DIM; i++)
    {
        double sum = FT_in[i];
        for (int j = 0; j < w - 1; j++) { sum += _FT_in_last[j][i]; }
        double W = w;
        FT_out[i] = sum / W;
        for (int j = w - 1; j >= 1; j--) { _FT_in_last[j][i] = _FT_in_last[j - 1][i]; }
        _FT_in_last[0][i] = FT_in[i];
    }
}

static void LowPassFilter(const double* FT_in, double* FT_out)
{
    for (int i = 0; i < 6; i++)
    {
        double RC = 0.0;
        double cof1 = 0.0;
        RC = 40.0 / filter / 2.0 / 3.14159265;
        cof1 = 1.0 / (1.0 + RC / (0.002));
        FT_out[i] = cof1 * FT_in[i] + (1 - cof1) * FT_last[i];
        FT_last[i] = FT_out[i];
    }
}

static enum CONNECT_STATE check_Sensor_config()
{
    enum CONNECT_STATE res = CONNECT_NO_ERROR;
    if (sensorShmStruct)
    {
        if (last_mode == 0 && sensorMode() == 1)
        {  // 0->1
            printf("SENSORMODULE:>> entering maintenance mode!\n");
            // 更新物理设置信息
            // sensor->setIp(sensorShmStruct->sensorConfig.commCfg.tcpComm.ipaddr);
            // sensor->setPort(sensorShmStruct->sensorConfig.commCfg.tcpComm.port);
            // sensor->setUSBDeviceName();
            if (sensor->InitSensor() == -1)
            {
                res = CONNECT_ERRRO;  // 连接失败
            }
            else
            {
                res = CONNECT_SUCC;
            }
        }
        else if (last_mode == 1 && sensorMode() == 0)
        {  //1->0
            printf("SENSORMODULE:>> entering maintenance mode!\n");
            sensor->CloseSensor();
            res = CONNECT_CLOSE;
        }
    }
    last_mode = sensorMode();  // 更新模式位
    return res;
}

void* TORQSENSOR_mgr(void* arg)
{
    printf("sensor brand = %d, enther chid thread\n", sensorBrand());
    while (!quit_flag)
    {
        if (sensor == nullptr || sensorShmStruct == nullptr)
        {
            return (void*)0;
        }
        int brand = sensorBrand();  // 1,2,3,4,5,7型
        brand = (1 << brand) & sensor->SensorBrand();
        if (!brand)
        {
            printf("Change Sensor Brand : from %d tp %d\n", sensor->SensorBrand(), sensorBrand());
            // 切线程
            sensor->CloseSensor();
            setSensorConnectClose();
            return (void*)0;
        }

        // 检测是否连接
        switch (check_Sensor_config())
        {
        case CONNECT_SUCC:
            setSensorConnectSucc();
            break;
        case CONNECT_ERRRO:
            setSensorInitError();
            break;
        case CONNECT_CLOSE:
            setSensorConnectClose();
            break;
        case CONNECT_TIMEOUT:
            setSensorInitError();
            break;
        default:
            break;
        }

        if (sensorStatus() == 1)
        {
            double sensor_data[CART_DIM] = {};
            static double FT_filter[CART_DIM] = {};
            switch (sensor->GetSensorData(sensor_data))  // 串口接收数据
            {
            case READ_SUCC:
                filter = sensorFilter();
                if (filter > 0.0)
                {
                    LowPassFilter(sensor_data, FT_filter);
                    setSensorData(FT_filter);
                }
                else
                {
                    // @FIXME 关闭滤波器，考虑是不是把状态变量清零
                    setSensorData(sensor_data);
                }
                break;
            case READ_WRONG_DATA:
                setSensorReadWrongData();
                break;
            case READ_NO_DATA:
                setSensorReadNoData();
                break;
            }
        }

        usleep(2 * 1000);
    }
    return (void*)0;
}

static void quit_signal(int signal)
{
    quit_flag = 1;
    exit(0);
}

int initSensorSvr()
{
    int shm_id = shmget(torqSensorKey, sizeof(SensorShm), IPC_CREAT | 0666);
    if (shm_id < 0)
    {
        printf("get id error\n");
        printf("torqsensormgr exited!\r\n");
        return -1;
    }

    sensorShmStruct = (SensorShm*)shmat(shm_id, NULL, 0);
    if (sensorShmStruct == NULL)
    {
        printf("get ptr error\n");
        printf("torqsensormgr exited!\r\n");
        return -1;
    }
    return 0;
}

int runSensorSvr()
{
    signal(SIGINT, quit_signal);
    signal(SIGTERM, quit_signal);
    if (sensorShmStruct)
    {
        while (!quit_flag)
        {
            pthread_t thread_sensormgr;
            pthread_attr_t attr_thread_sensormgr;

            /* Init to default values */
            if (pthread_attr_init(&attr_thread_sensormgr))
            {
                printf("Can not init thread_sensormgr attribute \r\n");
            }

            /* Set the requested stacksize for this thread */
            if (pthread_attr_setstacksize(&attr_thread_sensormgr, PTHREAD_STACK_MIN + MY_STACK_SIZE))
            {
                printf("Can not set stack size for thread_sensormgr \r\n");
            }

            int brand = sensorBrand();  // 6型不在这里处理
            if (brand > 0 && brand < MAX_SUPPORT_BRAND && brand != TIO_SENSOR_BRAND && !sensor)
            {
                // sensor = new ForceSensor(
                //     (FORCESENSORTYPE)brand, sensorShmStruct->sensorConfig.commCfg.tcpComm.ipaddr, sensorShmStruct->sensorConfig.commCfg.tcpComm.port);
            }

            /* Start the actual thread */
            if (sensor)
            {
                pthread_create(&thread_sensormgr, &attr_thread_sensormgr, TORQSENSOR_mgr, NULL);
                pthread_join(thread_sensormgr, NULL);
                pthread_attr_destroy(&attr_thread_sensormgr);
            }
            // 切换传感器后，退出线程
            delete sensor;
            sensor = nullptr;
            usleep(4000);
        }
    }
    return 0;
}
